#! /usr/bin/env python2

# Primary Author: Mark Culleton
# Email: culletom@tcd.ie

# Co-Author: Raymond Barrett
# Email: barretrj@tcd.ie

#import cv  
import time
import os
import math
import random
import pygame
import pygame.camera
from pygame.locals import *
import pyganim
from wordle import WordleWord
from frame_get import get_frame, string2Surface
import cv2.cv as cv
import cv2
import Image
import numpy as np
from Gestures import GestureTracker

NUM_FISH = 5
DEPTH = 30
# SCREENSIZE = [1900,1080]
SCREENSIZE = [1024,800]

DRAW = cv.CreateImage(SCREENSIZE, cv.IPL_DEPTH_8U, 3)

smallres = (SCREENSIZE[0]/5, SCREENSIZE[1]/5)
smallscreenpos = (SCREENSIZE[0]-smallres[0]-10,SCREENSIZE[1]-smallres[1]-10)
GRAVITY = 9.81  #m/s^2
BUYOANCY = 9.2    #m/s^2
WATER_RESIS = 0.2 #L/s

WORDS = ['Images/Words/word%s.png'%(n) for n in xrange(NUM_FISH)]
COLLISIONS = [0 for n in xrange(NUM_FISH)]
print COLLISIONS

FISH_POSITIONS = []
FISH_DIRECTIONS = []

cascade = cv.Load("haarcascade_frontalface_default.xml")
image_res2 = (512, 400)
COUNT = 2

DETECTSIZE = (SCREENSIZE[0]/6,SCREENSIZE[1]/6)
POINTERSIZE = [24,34]
#============================================================================================================================
def surface_to_string(surface):
    """Convert a pygame surface into string"""
    return pygame.image.tostring(surface, 'RGB')
  
def pygame_to_cvimage(surface):
    """Convert a pygame surface into a cv image"""
    image_string = surface_to_string(surface)
    cv_image = cv.CreateImage(surface.get_size(), cv.IPL_DEPTH_8U, 3)
    cv.SetData(cv_image, image_string)
    return cv_image
 
def string_to_cvimage(string):
    """Convert a pygame surface into a cv image"""
    cv_image = cv.CreateImage(image_res2, cv.IPL_DEPTH_8U, 3)
    cv.SetData(cv_image, string)
    image_bgr = cv.CreateMat(cv_image.height, cv_image.width, cv.CV_8UC3)
    cv.CvtColor(cv_image, image_bgr, cv.CV_RGB2BGR)
    big_image = cv.CreateImage(SCREENSIZE, cv.IPL_DEPTH_8U, 3)
    cv.Resize(image_bgr, big_image)
    return big_image

def string_to_cv2image(string):
    """Convert image string to CV2 image"""
    #print string
    img = np.fromstring(string, dtype=np.uint8)
    img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
    #print img
    #print type(img)
    return img

def cvimage_grayscale(cv_image):
    """Converts a cvimage into grayscale"""
    grayscale = cv.CreateImage(cv.GetSize(cv_image), 8, 1)
    cv.CvtColor(cv_image, grayscale, cv.CV_RGB2GRAY)
    return grayscale
 
def cvimage_to_pygame(image):
    """Convert cvimage into a pygame image"""
    image_rgb = cv.CreateMat(image.height, image.width, cv.CV_8UC3)
    cv.CvtColor(image, image_rgb, cv.CV_BGR2RGB)
    return pygame.image.frombuffer(image.tostring(), cv.GetSize(image_rgb),"RGB").convert()
 
def detect_faces(cv_image, storage):
    """Detects faces based on haar. Returns points"""
    small_image = cv.CreateImage(DETECTSIZE, cv.IPL_DEPTH_8U, 3)
    cv.Resize(cv_image, small_image)
    return cv.HaarDetectObjects(cvimage_grayscale(small_image), cascade,storage,1.1,3,cv.CV_HAAR_DO_CANNY_PRUNING,(40,40))

def draw_from_points(cv_image, points):
    """Takes the cv_image and points and draws a rectangle based on the points. Returns a cv_image."""
    for (x, y, w, h), n in points:
        new_x, new_y, new_w, new_h = scale_points(x,y,w,h)
        cv.Rectangle(cv_image, (new_x, new_y), (new_x + new_w, new_y + new_h), (0,255,0),2,4,0)
    return cv_image

def scale_points(x,y,w,h):
    new_x = int((float(x)/float(DETECTSIZE[0])) * SCREENSIZE[0])
    new_y = int((float(y)/float(DETECTSIZE[1])) * SCREENSIZE[1])
    new_w = int((float(w)/float(DETECTSIZE[0])) * SCREENSIZE[0])
    new_h = int((float(h)/float(DETECTSIZE[1])) * SCREENSIZE[1])
    return (new_x, new_y, new_w, new_h)

def FaceAndGestureRecognition(string):
    # Convert surface to cv image
    # cv_image = pygame_to_cvimage(string)
    cv_image = string_to_cvimage(string)
    cv2_image = np.asarray(cv_image[:,:])

    global DRAW
    DRAW = np.asarray(DRAW[:,:])

    gesture = 0
    frame,DRAW,gesture,point,whiteboard = GestureTracker().gestures(cv2_image, DRAW)
    #print point
    
    storage = cv.CreateMemStorage(0)  # Create storage
    points = detect_faces(cv_image, storage)  # Get points of faces.

    return (points, point, gesture)
#============================================================================================================================

class Fish(pygame.sprite.Sprite):
    def __init__(self, num, word):    
        pygame.sprite.Sprite.__init__(self)
        self.num = num
        self.angle = 0.0
        self.desired_angle = 0.0
        self.count = 0                  # Used to control duration of frames in sprite animation
        self.point = (0,0)              # Location of mouse click
        self.screensize = SCREENSIZE
        self.x_dir = 1; self.y_dir = 1; self.z_dir = 1  # X-Direction(+1:Left-to-Right, -1: Right-to-Left)     Y-Direction (+1: Down, -1: Up)            
        self.d_x = 0; self.d_y = 0; self.d_z = 0
        self.accum_z = 0
        self.face_counter = 0

        self.sideward_images = []
        self.turn_images = []
        self.backward_images = []
        self.forward_images = []
        self.half_turn_forward = []
        self.half_turn_backward = []

        self.clock = pygame.time.Clock()
        self.path = 'Images/Fish_GIF/Fish%s/'%self.num
        self.word = WordleWord(word)
        self.Detected_Face = False
        self.reacted_to_face = False

        self.Random_Direction = False
        self.Fish_Click = False
        self.changed_dir = False
        self.Turn = False
        self.Swim_Forward = False
        self.Swim_Backward = False
        self.Transparent = False      
        self.rotated_forward = False
        self.avoiding_fish = False


        self.motion = 'Turn'
        path, dirs, self.turn_frames = os.walk(self.path + self.motion).next()
        for i in xrange(1, len(self.turn_frames)):           # Create each fish sprite ('filename of each frame',    duration_of_frame_in_seconds)
            self.turn_images.append((self.path + self.motion + '/frame_%s.png'%(i)))

        halfway = int(len(self.turn_images)/2)
        for i in xrange(halfway,len(self.turn_images)-1):
            self.half_turn_backward.append(self.turn_images[i])

        self.motion = 'Backwards'
        path, dirs, self.backward_frames = os.walk(self.path + self.motion).next()
        for i in xrange(1, len(self.backward_frames)):           # Create each fish sprite ('filename of each frame',    duration_of_frame_in_seconds)
            self.backward_images.append((self.path + self.motion + '/frame_%s.png'%(i)))        

        self.motion = 'Forwards'
        path, dirs, self.forward_frames = os.walk(self.path + self.motion).next()
        for i in xrange(1, len(self.forward_frames)):           # Create each fish sprite ('filename of each frame',    duration_of_frame_in_seconds)
            self.forward_images.append((self.path + self.motion + '/frame_%s.png'%(i)))

        self.motion = 'Sidewards'
        path, dirs, self.forward_frames = os.walk(self.path + self.motion).next()
        for i in xrange(1, len(self.forward_frames)):           # Create each fish sprite ('filename of each frame',    duration_of_frame_in_seconds)
            self.sideward_images.append((self.path + self.motion + '/frame_%s.png'%(i)))        
        
        self.fsh = pyganim.PygAnimation(self.sideward_images, 0.05)
        
        self.width, self.height = self.fsh.getMaxSize()

        self.x_pos = random.uniform(self.width,self.screensize[0]-self.width)
        self.y_pos = random.uniform(self.height,self.screensize[1]-self.height)
        self.z_pos = random.randint(1,DEPTH)
                
        self.x_dir = random.choice([-1,1])
        #self.y_dir = random.choice([-1,1])
        #self.z_dir = random.choice([-1,1])
        
        self.impulse_vel = random.uniform(60, 100)       # Initial Velocity of fish caused by fin motion
        self.angle = random.uniform(-30,30) * math.pi / 180   # Angle fish swims (In Radians)
        
        self.x_speed = self.x_dir*(self.impulse_vel * math.cos(self.angle)) # Magnitude of Velocity in x-direction
        self.y_speed = (self.impulse_vel * math.sin(self.angle)) # Magnitude of Velocity in y-direction
        self.z_speed = random.uniform(-10,10)

        self.y_dir = math.copysign(1, self.y_speed)

        self.x_acc = -(WATER_RESIS * self.x_speed)
        self.z_acc = -(WATER_RESIS * self.z_speed)
        
        ###NOTE: '-' implies fish moving up and '+' implies fish moving down
        if self.y_speed > 0:        #Fish moving Downwards  (y_speed is +ive)      
            self.y_acc = -(WATER_RESIS * self.y_speed) - (GRAVITY-BUYOANCY)
        else:                       #Fish moving Upwards  (y_speed is -ive)    
            self.y_acc = -(WATER_RESIS * self.y_speed) + (GRAVITY-BUYOANCY)  
                
        if self.x_dir < 0:
            self.fsh.flip(True, False)
        
        self.crit_vel = self.impulse_vel / 2.5           # Velocity at which fish flaps again        
        FISH_POSITIONS.append((self.x_pos, self.y_pos))    #Globally store each fish location
        FISH_DIRECTIONS.append((self.x_dir,self.y_dir))    #Globally store each fish location

        # self.fsh.rotate(-self.angle) 
     
        self.fsh.play()                 # Begins playing the fish sprite
                
    # Called in every iteration of the "game" loop, updates each fish.
    def update(self): 
        self.time_passed = self.clock.tick()    #unit is milliseconds
        self.time = self.time_passed / 1000.0   #unit is seconds 

        if (self.x_pos + self.width > self.screensize[0] and self.x_dir > 0) or (self.x_pos < 0 and self.x_dir < 0): #or (random.randint(0,500) == 10): #Fish go off edges before turning. Occasionally the fish turns randomly 
            if (self.y_pos + self.height > self.screensize[1]-40 and self.y_dir > 0) or (self.y_pos < 0 and self.y_dir < 0):
                self.y_dir *= -1
                self.y_speed *= -1
                self.desired_angle = self.angle * -1                
            if self.Swim_Forward:
                self.x_speed *= -1
                self.x_dir *= -1
            else:
                self.Turn = True          
        elif (self.y_pos + self.height > self.screensize[1]-40 and self.y_dir > 0) or (self.y_pos < 0 and self.y_dir < 0):
            self.y_dir *= -1
            self.y_speed *= -1
            self.desired_angle = self.angle * -1
        else:
            pass
            # self.check_for_collision()
        
        if self.z_pos >= DEPTH or self.z_pos <= 0:
            self.z_speed *= -1
            self.z_dir *= -1
            # if self.num < 5:
            #     if math.fabs(self.z_pos) >= DEPTH:
            #         self.Swim_Forward = True
            #     else:
            #         self.Swim_Backward = True

        
        self.d_x = (self.x_speed * self.time) + (0.5 * self.x_acc * math.pow(self.time,2))  #Current displacement is calculated using previously recorded values
        self.d_y = (self.y_speed * self.time) + (0.5 * self.y_acc * math.pow(self.time,2)) 
        self.d_z = (self.z_speed * self.time)
        # self.accum_z += math.fabs(self.d_z)
        if self.Turn:
            # #print "TURNING"
            self.turn()        
        elif self.Swim_Forward:
            #print "WENT IN HERE"
            self.swim_forward()

        elif self.Detected_Face and not self.reacted_to_face:            
            # #print "RIGHT_CLICK"
            self.detected_face()

        # elif self.Fish_Click:
        #     # #print "FISH_CLICK"
        #     self.fish_click()
        # elif self.Random_Direction:
        #     # #print "RANDOM_DIRECTION"
        #     self.random_direction()
        else:
            # #print "NORMAL_SWIM"
            self.normal_swim()
        #self.rotate()
        # if self.accum_z > 0.5:
        #     print 'UPDATED', self.accum_z
        # if self.Swim_Forward or self.Swim_Backward:
        # self.scale()            
            # self.accum_z = 0      
        #self.fsh.getRect().center = self.oldCenter
        self.x_pos += self.d_x
        self.y_pos += self.d_y
        self.z_pos += self.d_z
        #print self.z_pos

        print self.x_dir

        FISH_POSITIONS[self.num] = (self.x_pos,self.y_pos)   
        FISH_DIRECTIONS[self.num] = (self.x_dir,self.y_dir)   
        
    def normal_swim(self):            
        if math.fabs(self.x_speed) < self.crit_vel: #Fish is now swimming too slow
            self.desired_angle = random.uniform(-30,30) * math.pi / 180   # Angle fish swims (In Radians)
            self.impulse_vel = random.uniform(60,100)
            self.x_speed = self.x_dir*(self.impulse_vel * math.cos(self.desired_angle)) # Magnitude of Velocity in x-direction
            self.y_speed = self.impulse_vel * math.sin(self.desired_angle) # Magnitude of Velocity in y-direction
            self.y_dir = math.copysign(self.y_dir, self.y_speed)
            self.z_speed = random.uniform(-10,10)            
            self.count = 0      # Reset the counter
            self.fsh.rate = 1   # Speed up animation frames to coincide with increased speed                         
        else:
            if self.count > 10:     # Once count has exceeds 10, animation returns to normal speed 
                self.fsh.rate = 0.4      
            self.x_speed += self.time*self.x_acc    # v=u+at
            self.y_speed += self.time*self.y_acc    # Since up is negative and down is positive 
            self.z_speed += self.time*self.z_acc

            #self.angle = math.atan2(math.fabs(self.y_speed), math.fabs(self.x_speed))
            #self.fsh.rotate(self.angle * -self.y_dir)
            self.count += 1
            
        self.x_acc = -(WATER_RESIS * self.x_speed)
        self.z_acc = -(WATER_RESIS * self.z_speed)
        if self.y_speed > 0:        #Fish moving Downwards  (y_speed is +ive)      
            self.y_acc = -(WATER_RESIS * self.y_speed) - (GRAVITY-BUYOANCY)
        else:                       #Fish moving Upwards  (y_speed is -ive)    
            self.y_acc = -(WATER_RESIS * self.y_speed) + (GRAVITY-BUYOANCY)  

        
    def check_for_collision(self):
        for i in range(len(FISH_POSITIONS)):
            if self.num <= i:
                x_diff = math.fabs(self.x_pos - FISH_POSITIONS[i][0])
                y_diff = math.fabs(self.y_pos - FISH_POSITIONS[i][1])
                if (x_diff < self.width * 1.2) and (y_diff < self.height):
                    self.collisions[i] = 1

        for i in range(len(self.collisions)):
            if self.collisions[i] != 0 and not self.avoiding_fish:
                x_diff = math.fabs(self.x_pos - FISH_POSITIONS[i][0])
                y_diff = math.fabs(self.y_pos - FISH_POSITIONS[i][1])                    
                self.avoiding_fish = True
                if self.Swim_Forward:
                    self.x_speed *= -1
                    self.x_dir *= -1
                elif self.Turn == False:
                    if self.x_dir == FISH_DIRECTIONS[i][0]:
                        if (self.y_pos < FISH_POSITIONS[i][1]):  #Fish is above other fish                  
                            self.y_dir = -1
                            self.y_speed = math.copysign(self.y_speed, self.y_dir)
                        else:
                            self.y_dir = 1
                            self.y_speed = math.copysign(self.y_speed, self.y_dir)                            

                    elif (y_diff < self.height/2):
                        #print "Needs to Turn to Avoid Collision?"
                        self.Turn = True
                    elif (self.y_pos < FISH_POSITIONS[i][1]):  #Fish is above other fish                  
                        self.y_dir = -1
                        self.y_speed = math.copysign(self.y_speed, self.y_dir)
                    else:
                        self.y_dir = 1
                        self.y_speed = math.copysign(self.y_speed, self.y_dir)
                else:
                    pass 
        self.avoiding_fish = False
        
        print self.num, 'collisions:', collisions
        print self.num, 'x_pos:', self.x_pos
        print self.num, 'y_pos:', self.y_pos

    def swim_to_point(self, points):
        self.face_points = []
        # self.face_rects = []
        for i in range(len(points)):
            p, _ = points[i]        #At present only reacts to one face
            x,y,w,h = p
            x,y,w,h = scale_points(x,y,w,h)
            # #print x,y,w,h
            x_diff = x + w/2
            y_diff = y + h/2
            mouse_point = (x_diff,y_diff)
            #mouse_point = (x,y)
            self.face_points.append(mouse_point)
            self.face_rect = ((x,y),(x+w,y+h))
        self.Detected_Face = True                

       
    def detected_face(self):     
        # weighting = []
        # closest = 0
        # normaliser = 0
        # rand_dir_prob = 0.2
        # for i in range(len(self.face_points)):
        #     weighting.append(self.face_rects[i][0])
        #     normaliser += self.face_rects[i][0]
        #     if math.fabs(self.x_pos - self.face_points[i][0]) < math.fabs(self.x_pos - self.face_points[0][0]):
        #         closest = i
        # weighting[closest] *= 2
        # normaliser += weighting[closest]

        # weighting.append(weighting[closest]*rand_dir_prob)
        # normaliser += weighting[closest]*rand_dir_prob

        # for i in range(len(weighting)):
        #     weighting[i] /= normaliser

        # print weighting

        # choice = random.uniform(0,1)

        if self.x_pos > (self.face_rect[0][0]+20) and self.x_pos < (self.face_rect[1][0]-20) and self.y_pos > (self.face_rect[0][1]+20) and self.y_pos < (self.face_rect[1][1]-20):
            print "Within Face Area"
            self.Swim_Forward = True
            self.reacted_to_face = True

        else:
            if self.Turn:
                print "Turning"
            
            elif math.fabs(self.x_speed) < 2* self.crit_vel: #Fish is now swimming too slow
                print "Impulsing towards face"
                self.desired_angle = math.atan2((self.face_points[0][1] - self.y_pos),(self.face_points[0][0] - self.x_pos))    #Angle between two points
                print "Desired Angle", self.desired_angle *180/math.pi
                self.impulse_vel = random.uniform(60,100) * 1.5
                if math.fabs(self.desired_angle * 180/math.pi) > 170:
                    self.desired_angle = 0
                    if self.x_dir > 0:
                        self.Turn = True
                
                elif math.fabs(self.desired_angle * 180/math.pi) > 90:
                    if self.desired_angle < 0:
                         self.desired_angle += (90*math.pi/180)
                    else:
                         self.desired_angle -= (90*math.pi/180) 

                    if self.x_dir > 0:
                        self.Turn = True
                
                else:
                    if self.x_dir < 0:
                        self.Turn = True
                
                if not self.Turn:# if math.fabs(self.desired_angle * 180/math.pi) > 30:
                #     self.desired_angle = math.copysign(30*math.pi/180,self.desired_angle)
                #print self.desired_angle * 180/math.pi     
                    self.x_speed = self.x_dir*(self.impulse_vel * math.cos(self.desired_angle)) # Magnitude of Velocity in x-direction
                    self.y_speed = self.impulse_vel * math.sin(self.desired_angle) # Magnitude of Velocity in y-direction
                    self.z_speed = random.uniform(-10,10)            
                    self.count = 0      # Reset the counter
                    self.fsh.rate = 1   # Speed up animation frames to coincide with increased speed                         
            
            else:
                print "Gliding"
                if self.count > 10:     # Once count has exceeds 10, animation returns to normal speed 
                    self.fsh.rate = 0.4      
                self.x_speed += self.time*self.x_acc    # v=u+at
                self.y_speed += self.time*self.y_acc    # Since up is negative and down is positive 
                self.z_speed += self.time*self.z_acc

                #self.angle = math.atan2(math.fabs(self.y_speed), math.fabs(self.x_speed))
                #self.fsh.rotate(self.angle * -self.y_dir)
                self.count += 1
                
                self.x_acc = -(WATER_RESIS * self.x_speed)
                self.z_acc = -(WATER_RESIS * self.z_speed)
                if self.y_speed > 0:        #Fish moving Downwards  (y_speed is +ive)      
                    self.y_acc = -(WATER_RESIS * self.y_speed) - (GRAVITY-BUYOANCY)
                else:                       #Fish moving Upwards  (y_speed is -ive)    
                    self.y_acc = -(WATER_RESIS * self.y_speed) + (GRAVITY-BUYOANCY)    
    
    def random_direction(self): # Called once right mouse button is released (fish to begin swimming again)
        new_x_dir = random.choice([-1,1])
        self.desired_angle = random.uniform(-30,30) * math.pi / 180   # Angle fish swims (In Radians)
        self.x_speed = new_x_dir*(self.impulse_vel * math.cos(self.desired_angle)) # Magnitude of Velocity in x-direction
        self.y_speed = self.impulse_vel * math.sin(self.desired_angle) # Magnitude of Velocity in y-direction
        self.z_speed = random.uniform(-10,10)
        self.y_dir = math.copysign(1, self.y_speed)
        #self.rotate() 
        if (new_x_dir != self.x_dir): # If fish is now swimming in the opposite direction
            self.fsh.flip(True, False)
            self.x_dir *= -1
        self.Random_Direction = False       # Only occurs once after right mouse button is released 

    def fish_click(self):   # If a fish is clicked (Currently only flips fish, but could be developed)
         self.turn()


    def turn(self):
        if self.motion != 'Turn':
            self.motion = 'Turn'
            self.Turn = True
            self.fsh.change_animation(self.turn_images, 0.02, loop = False)
            self.fsh.rate = 1.2      
            self.z_speed = 0
            self.changed_dir = False
            if self.x_dir < 0:
                self.fsh.flip(True, False)

        current_frame = self.fsh.current_frame_num
        
        if current_frame < len(self.turn_images) / 2 and not self.changed_dir:
            self.x_speed *= 0.5
            # #print "IN LESS THAN"        
        
        else:
            # #print "IN ELSE"
            if not self.changed_dir:
                self.x_speed *= -1
                self.x_speed = math.copysign(self.impulse_vel * math.cos(self.desired_angle),self.x_speed)
                self.changed_dir = True 
                # #print "CHANGED DIRECTION"

            if self.fsh.state == 'stopped':
                # #print "Changed to Forward Animation"
                self.x_dir *= -1
                self.z_speed = random.uniform(-10,10)
                self.motion = 'Sidewards'
                self.fsh.change_animation(self.sideward_images, 0.05, loop = True)
                self.fsh.rate = 1      
                self.interact = False
                self.Turn = False
                if self.x_dir < 0:
                    self.fsh.flip(True, False)                                                        
        
        self.x_acc = -(WATER_RESIS * self.x_speed)
        if self.y_speed > 0:        #Fish moving Downwards  (y_speed is +ive)      
            self.y_acc = -(WATER_RESIS * self.y_speed) - (GRAVITY-BUYOANCY)
        else:                       #Fish moving Upwards  (y_speed is -ive)    
            self.y_acc = -(WATER_RESIS * self.y_speed) + (GRAVITY-BUYOANCY)   

    def swim_forward(self):
        if self.motion != 'Turn' and self.rotated_forward == False:
            self.motion = 'Turn'
            self.fsh.change_animation(self.turn_images, 0.02, loop = False)
            self.fsh.rate = 1.2      
            self.rotated_forward = False
            self.rotated_sideward = False
            if self.x_dir < 0:
                self.fsh.flip(True, False)

        current_frame = self.fsh.current_frame_num
        
        if current_frame < len(self.turn_images) / 2 and not self.rotated_forward:
            self.x_speed *= 0.5
            # #print "IN LESS THAN"                
        else:
            if not self.rotated_forward:
                self.fsh.change_animation(self.forward_images, 0.05, loop = True)
                self.fsh.rate = 1                      
                self.motion = 'Forwards'
                self.rotated_forward = True 
            
            elif self.z_pos <= 2:
                if not self.rotated_sideward:
                    self.fsh.change_animation(self.half_turn_backward, 0.05, loop = False)
                    self.fsh.rate = 1.2                      
                    self.motion = 'Half_Turn'
                    self.rotated_sideward = True
                    if self.x_dir > 0:
                        self.fsh.flip(True, False)

                if self.fsh.state == 'stopped':
                    # #print "Changed to Forward Animation"
                    self.motion = 'Sidewards'
                    self.fsh.change_animation(self.sideward_images, 0.05, loop = True)
                    self.fsh.rate = 1      
                    self.desired_angle = random.uniform(-30,30) * math.pi / 180   # Angle fish swims (In Radians)
                    self.impulse_vel = random.uniform(60,100)
                    self.x_speed = self.x_dir*(self.impulse_vel * math.cos(self.desired_angle)) # Magnitude of Velocity in x-direction
                    self.y_speed = self.impulse_vel * math.sin(self.desired_angle) # Magnitude of Velocity in y-direction-1

                    self.rotated_forward = False
                    self.Swim_Forward = False
                    if self.x_dir < 0:
                        self.fsh.flip(True, False)                   
            
            else:                
                if math.fabs(self.x_speed) < self.crit_vel: #Fish is now swimming too slow
                    self.desired_angle = random.uniform(-10,10) * math.pi / 180   # Angle fish swims (In Radians)
                    self.impulse_vel = random.uniform(10,20)
                    self.x_speed = self.x_dir*(self.impulse_vel * math.cos(self.desired_angle)) # Magnitude of Velocity in x-direction
                    self.y_speed = self.impulse_vel * math.sin(self.desired_angle) # Magnitude of Velocity in y-direction
                    self.z_speed = math.copysign(random.uniform(5,10),self.z_speed)            
                    self.count = 0      # Reset the counter
                    self.fsh.rate = 1   # Speed up animation frames to coincide with increased speed                         
                else:
                    if self.count > 10:     # Once count has exceeds 10, animation returns to normal speed 
                        self.fsh.rate = 0.4      
                    self.x_speed += self.time*self.x_acc    # v=u+at
                    self.y_speed += self.time*self.y_acc    # Since up is negative and down is positive 
                    self.z_speed += self.time*self.z_acc

                    #self.angle = math.atan2(math.fabs(self.y_speed), math.fabs(self.x_speed))
                    #self.fsh.rotate(self.angle * -self.y_dir)
                    self.count += 1

        self.z_acc = -(WATER_RESIS * self.z_speed)        
        self.x_acc = -(WATER_RESIS * self.x_speed)
        if self.y_speed > 0:        #Fish moving Downwards  (y_speed is +ive)      
            self.y_acc = -(WATER_RESIS * self.y_speed) - (GRAVITY-BUYOANCY)
        else:                       #Fish moving Upwards  (y_speed is -ive)    
            self.y_acc = -(WATER_RESIS * self.y_speed) + (GRAVITY-BUYOANCY)   

    def scale(self):
        #self.multiplier = self.z_pos/20
        # self.width, self.height = self.fsh.getMaxSize()
        self.width, self.height = self.fsh.getMaxSize()
        #print self.width - int(self.z_pos)

        ratio = self.width/self.height
        self.fsh.scale((self.width - int(self.z_pos), self.height - int(self.z_pos/ratio)),self.motion,self.x_dir)
        # if self.transparent:
        #     self.fsh.make_transparent(0)
        # if self.x_dir < 0:
        #     self.fsh.flip(True, False)
    
    def rotate(self):
       # #print math.fabs(math.fabs(self.angle) - math.fabs(self.desired_angle))
        if math.fabs(self.angle - self.desired_angle) > 0.1:
            if self.desired_angle > self.angle:
                self.fsh.rotate(-(self.angle + 0.05))
                self.angle += 0.05
            else:
                self.fsh.rotate(-(self.angle - 0.05))
                self.angle -= 0.05
            if self.Transparent:
                self.fsh.make_transparent(0)
            if self.x_dir < 0:
                self.fsh.flip(True, False)
            
    def blit(self, screen): # Pastes fish onto screen
        self.screen = screen
        self.fsh.blit(screen, self.get_pos())
    
    def get_pos(self):
        return (self.x_pos, self.y_pos)

    def check_for_interaction(self, point):
        if (math.fabs(self.x_pos + self.width/2 - point[0]) < 50) and (math.fabs(self.y_pos + self.height/2 - point[1]) < 50):
            self.Fish_Click = True
            return True
        else:
            return False

    def left_release(self): 
        self.Fish_Click = False

    def make_transparent(self):
        self.fsh.make_transparent(0) # Calls the pyganim.py method
        self.Transparent = True

    def reset_colour(self):
        self.fsh.clearTransforms()  # Returns original images (which may need to be flipped)
        self.Transparent = False
        if self.x_speed < 0:
            self.fsh.flip(True, False)

class Bubble(pygame.sprite.Sprite):
    def __init__(self, word):    
        pygame.sprite.Sprite.__init__(self)
        self.clock = pygame.time.Clock()
        self.screensize = SCREENSIZE
        self.img = []
        
        for i in xrange(1, 26):
            self.img.append('Images/Bubble_GIF/Bubble2_%s.png'%(i))   # Load Each Frame
        self.bub = pyganim.PygAnimation(self.img, 0.2)

        self.width, self.height = self.bub.getMaxSize()

        self.bub.play()     # Play the Bubble Animation

    def blit(self, screen, pos):     # Paste Bubble animation onto screen
        self.bub.blit(screen, pos)
    
    def make_transparent(self):
        self.bub.make_transparent(0) 
        
    def reset_bubble(self):
        self.bub.clearTransforms()
        self.bub.scale([self.width*self.multiplier,self.height*self.multiplier])  # Make Bubbles go to top of screen

class VideoFishTank(object):
    def __init__(self, **argd):
        self.__dict__.update(**argd)
        super(VideoFishTank, self).__init__(**argd)
        self.screen = pygame.display.set_mode(SCREENSIZE)
        pygame.display.set_caption('The SAP Fish Tank') 
        # pygame.display.toggle_fullscreen()
        self.orig_air_bubble = pygame.image.load('Images/Bubble1.png').convert_alpha()
        p = pygame.image.load('Images/mouse-cursor-icon.png').convert_alpha()
        self.pointer = pygame.transform.scale(p, POINTERSIZE)

        self.click_count = 0

        self.emit_word = []
        self.transparency = []
        self.air_bubble = []
        self.bubble_location = []
        self.bubble_size = []
        self.word = []
        for i in range(NUM_FISH):
            self.emit_word.append(False)
            self.transparency.append(255)
            self.air_bubble.append(self.orig_air_bubble)
            self.bubble_size.append(5)
            self.bubble_location.append([0,0])
            self.word.append(pygame.image.load(WORDS[i]).convert_alpha())

        self.clock = pygame.time.Clock()
        self.count = COUNT
    self.transparent_bub = False

    def setup_fishtank(self):   # These remain stationary, therefore can be recalled when resetting fish tank transparency
        ###CODE FOR ANIMATED OCEAN IF USED
        ocean_path = 'Images/Ocean_GIF/Ocean1/'
        ocean_images = []
        path, dirs, ocean_frames = os.walk(ocean_path).next()
        for i in xrange(1, len(ocean_frames)):           # Create each fish sprite ('filename of each frame',    duration_of_frame_in_seconds)
            ocean_images.append(ocean_path + 'frame_%s.png'%(i))
        self.ocean = pyganim.PygAnimation(ocean_images, 0.1)
        self.ocean.scale(SCREENSIZE, 'Ocean', 1)
        self.ocean.make_transparent(100)
        self.ocean.play()
        #self.ocean = pygame.image.load('Images/Ocean_1.png').convert_alpha()    # Load image
        #self.ocean = pygame.transform.scale(self.ocean,SCREENSIZE)      # Ensure image is same size as screen
        #self.ocean.fill((255, 255, 255, 170), None, pygame.BLEND_RGBA_MULT)  # Make slightly transparent (170) to allow for the live video stream to be seen 
        
        #self.depth = []                        
        #for i in range(50):
        #    self.depth.append(pygame.Surface(SCREENSIZE,pygame.SRCALPHA, 32).convert_alpha())
        
        self.foreground = pygame.image.load('Images/Fish_Tank_Bed2.png').convert_alpha() 
        self.foreground = pygame.transform.scale(self.foreground,(SCREENSIZE[0],SCREENSIZE[1]))
        self.foreground.fill((255, 255, 255, 255), None, pygame.BLEND_RGBA_MULT)
        
    def setup_fish(self):   #Can only be used for initial setup, as fish are moving 
        self.fish = [Fish(n, 'word%d' % n) for n in xrange(NUM_FISH)]    #Load the chosen number of fish

    def setup_bubbles(self):
        self.bubble = Bubble(None)  

    def make_transparent(self): #Makes full fish tank overlay transparent
        self.foreground.fill((255, 255, 255, 0), None, pygame.BLEND_RGBA_MULT)
        #self.ocean.fill((255, 255, 255, 150), None, pygame.BLEND_RGBA_MULT) 
        self.ocean.make_transparent(0)
        self.bubble.make_transparent()
        for fsh in self.fish:
            fsh.make_transparent()
            
    def reset_tank(self):   #Remove transparency and return fish tank overlay
        self.setup_fishtank()
        self.bubble.reset_bubble()
        for fsh in self.fish:
            fsh.reset_colour()  
    
    def update(self, queue):   #Iterative Loop
        point = []
        previouscreen = None
        smallscreen = None
        #for i in range(50):
        #    self.depth[i].fill(255)
        #    self.depth[i].set_alpha(255)

        data = get_frame(self.address)

        # if self.count > 0:
        #     background = string2Surface(data)
        #     self.count -= 1
        # else:
        #     background = faceRecogStuff(data)
        #     self.count = COUNT
        background = string2Surface(data)

        # Pull local frame off queue
        if queue.full():
            smallscreen = queue.get()
            points, point, gesture = FaceAndGestureRecognition(smallscreen)
            smallscreen = string2Surface(smallscreen)

            if gesture == 0 and self.state == 'tank':
                # print gesture, self.state
                self.check_event(point)
            elif gesture == 5 and self.state == 'word':
                # close wordle
                #print gesture, self.state
                self.check_event((0,0))
                print "GESTURE 5"
            elif gesture == 5 and self.state == 'user':
                # close wordle
                #print gesture, self.state
                self.check_event((0,0))
                #print "GESTURE 5"

            elif gesture == 3 and self.state == 'word':
                # print gesture, self.state
                #print "GESTURE 3"
                self.check_event(point)
            print gesture, self.state

            # Make it the smaller size
            smallscreen = pygame.transform.scale(smallscreen, smallres)
        else:
            smallscreen = previouscreen

        #Paste all layers on screen
        self.screen.blit(background,(0,0))
        #self.screen.blit(self.ocean,(0,0))
        self.ocean.blit(self.screen,(0,0))    # Used if Ocean is animated           
        self.screen.blit(self.foreground,(0,0))
        self.bubble.blit(self.screen, (100,0))
        #for i in range(50):
        #    self.screen.blit(self.depth[i], (0,0))
        
        if random.randint(0,10) == 5 and not self.transparent_bub:
            self.choice = random.randint(0, NUM_FISH-1)
            self.chosen_fish = self.fish[self.choice]
            if self.chosen_fish.y_pos < SCREENSIZE[1]/3 or self.chosen_fish.x_pos > SCREENSIZE[0]-150 or self.chosen_fish.x_pos < 150:
                pass
            elif self.emit_word[self.choice] == True:
                pass
            else:
                self.transparency[self.choice] = 255
                self.emit_word[self.choice] = True
                self.bubble_size[self.choice] = 5
                if (self.chosen_fish.x_speed < 0):
                    self.bubble_location[self.choice] = [self.chosen_fish.x_pos, self.chosen_fish.y_pos + self.chosen_fish.height/2]
                else:
                    self.bubble_location[self.choice] = [self.chosen_fish.x_pos + self.chosen_fish.width, self.chosen_fish.y_pos + self.chosen_fish.height/2]

        for i in xrange(len(self.emit_word)):
            if self.emit_word[i]:
                self.emit(i)
              
        for fish in self.fish:
            fish.blit(self.screen)
            if points:
                print points
                fish.swim_to_point(points)
                fish.face_counter = 0
            elif fish.face_counter > 30:
                fish.Detected_Face = False
                fish.reacted_to_face = False
            else:
                fish.face_counter += 1
            print 'counter', fish.face_counter
            fish.update()

        if smallscreen:
            previouscreen = smallscreen
            self.screen.blit(smallscreen, smallscreenpos)
        elif previouscreen:
            self.screen.blit(previouscreen,smallscreenpos)

        if self.chk >= 0 and self.state == 'word':
            self.fish[self.chk].word.display(self.screen)
        elif self.chk >= 0 and self.state == 'user':
            print self.av
            self.fish[self.chk].word.associated[self.av].display(self.screen)

        if type(point) == tuple and gesture >= -1:
            self.screen.blit(self.pointer, point)

        self.clock.tick()
        # print self.clock.get_fps()

        pygame.display.update()   

    def emit(self, choice):
        self.air_bubble[choice] = self.orig_air_bubble
        self.air_bubble[choice] = pygame.transform.scale(self.air_bubble[choice], (self.bubble_size[choice],self.bubble_size[choice]))
        self.air_bubble[choice].fill((255, 255, 255, self.transparency[choice]), None, pygame.BLEND_RGBA_MULT)
        self.bubble_location[choice][1] -= 8 
        self.screen.blit(self.air_bubble[choice],self.bubble_location[choice])

        self.bubble_size[choice] += 2
        if self.bubble_size[choice] <= 10:
            self.bubble_location[choice][0] += self.fish[choice].d_x
        else:
            self.word[choice] = pygame.image.load(WORDS[choice]).convert_alpha()

            self.word[choice].fill((255, 255, 255, self.transparency[choice]), None, pygame.BLEND_RGBA_MULT)

            self.word_width, self.word_height = self.word[choice].get_size()
            self.bubble_width, self.bubble_height = self.air_bubble[choice].get_size()
            self.scaler = float(self.word_width) / float(self.bubble_size[choice])
            self.word[choice] = pygame.transform.scale(self.word[choice], (int(self.word_width / self.scaler), int(self.word_height / self.scaler)) )
            self.screen.blit(self.word[choice],(self.bubble_location[choice][0], self.bubble_location[choice][1] + (self.bubble_height/2 - int(self.word_height / self.scaler)/2))) #[0]- self.width/2, self.bubble_location[1] - self.height/2))
            if self.bubble_size[choice] >= 60:
                self.bubble_size[choice] -= 2
                self.transparency[choice] -= 10
            if self.transparency[choice] < 0:
                self.emit_word[choice] = False
    
    def check_event(self, event):
        i = 0
        self.av = -1
        if self.state == "tank":
            for i in  xrange(len(self.fish)):
                chk = self.fish[i].check_for_interaction(event)   #Check if click is over a fish
                #print "Chk, i, state:",chk,i, state
                if chk:
                    self.state, self.av = self.fish[i].word.change_state(self.state, event)
                    self.chk = i
        else:
            self.state, self.av = self.fish[self.chk].word.change_state(self.state, event)

    def main(self, addr, que):
        self.setup_fishtank()
        self.setup_fish()
        self.setup_bubbles()

        self.address = addr
        
        self.av = -1
        self.chk = -1
        self.state = 'tank'

        while True: #Game Loop - Detects Key Presses and Mouse Clicks 
            for event in pygame.event.get():
                if event.type == QUIT:
                    pygame.display.quit()
                    exit()
                elif pygame.key.get_pressed()[K_UP]:
                    self.make_transparent()
                    self.transparent_bub = True
                elif pygame.key.get_pressed()[K_DOWN]:
                    self.reset_tank()
                    self.transparent_bub = False
                elif pygame.key.get_pressed()[K_ESCAPE]:
                    pygame.display.quit()
                    exit()
                elif event.type == pygame.MOUSEBUTTONDOWN:
                    if event.button == 1:   #Left Clicks
                        self.check_event(event.pos)
                    # elif event.button == 3: #Right Click
                    #     for fish in self.fish:
                    #         fish.swim_to_point(event.pos)   #Get all fish to swim towards point
                # elif event.type == pygame.MOUSEBUTTONUP:    #After mouse button has been released
                #     if event.button == 1:
                #         for fish in self.fish:
                #             fish.left_release()                    

            self.update(que)
          
def main():  
    pygame.init()
    VideoFishTank().main()
    pygame.quit()

if __name__ == "__main__":
    main()
